#ifndef __DENSO_H__
#define __DENSO_H__


// DSInvKine.cpp : rewrite inverse kinematic solver for DENSO in Cpp
// v1.0 (02/09/12): created
// v2.0 (18/09/12): split to cpp, hpp and start coding


//#include "stdafx.h"

//#include <cv.h>
//#include <cxcore.h>
//#include <highgui.h>

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>

#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vector>
#include "matwork.hpp"
#include "RS232.h"

const double INF = 999999999999999;

using namespace cv;
using namespace std;


struct DHtable
{
	double a1, a2, a3, a4, a5, a6;
	double d1, d2, d3, d4, d5, d6;
	double alf1, alf2, alf3, alf4, alf5, alf6;
};


class DENSOrobot
{
private:

	RS232 serialPort;
	
	Mat DENSOrobot::RPY_DENSO(const Mat fnlpose);

	void DENSOrobot::cs4Cal_v20(double fi1, double fi2, double fi3, double fi5, double ax, double ay, double az, 
		double& cfi, double& sfi);

	void DENSOrobot::abCal_v20(double fi1, double fi2, double fi3, double nx, double ny, double nz, double ox, double oy, double oz, 
		double& A, double& B);

	void DENSOrobot::cs6Cal_v20(double A, double B, double fi5, 
		double& cfi, double& sfi);

	char DENSOrobot::checkSOL(Mat& joints);

	int DENSOrobot::checkOut(double* joints);

public:
	
	DHtable DH;

	int use_RS232;
	
	DENSOrobot();

	DENSOrobot(int use_RS232_flg);

	~DENSOrobot();

	void DENSOrobot::FKineSolve(const Mat fi, Mat& A);

	void DENSOrobot::IKineSolve(const Mat fnlpose, Mat& SOL);

	Mat DENSOrobot::rotx(double fi);

	Mat DENSOrobot::roty(double fi);

	Mat DENSOrobot::rotz(double fi);

	int DENSOrobot::TrajPlanner(const Mat OrgPose, const Mat DestPose, Mat& BestTraj);

};

#endif